#ifndef QUATERNION_H
#define QUATERNION_H
#include "typedefs.h"
#include <Eigen/Geometry>
#include <QString>
#include "matrix33.h"

namespace Math
{

class AngleAxis;


class Quaternion
{
#ifdef USE_FLOAT
    Eigen::Quaternionf q;
#else
    Eigen::Quaterniond q;
#endif
public:
    Quaternion();

    Quaternion(const Quaternion &v) { q = v.q;}

    Quaternion(const bool &identity) { if (identity) q = q.Identity(); }

    Quaternion(const Real &w,const  Real &x,const  Real &y,const Real &z) { q.w() = w; q.x() = x; q.y() = y; q.z() = z; }

    Quaternion(const AngleAxis &aa);

    void setIdentity(void) { q = q.Identity(); }

    Eigen::Quaternion<Real> getEigen(void) const { return q;}

    Real w() const{ return q.w(); }
    Real x() const{ return q.x(); }
    Real y() const{ return q.y(); }
    Real z() const{ return q.z(); }

    static Quaternion IDENTITY;

    Matrix33 toRotationMatrix(void) const { return q.toRotationMatrix(); }

    QString toString() const {
        QString res;
        QString sx,sy,sz,sw;
        sx.setNum(x());
        sy.setNum(y());
        sz.setNum(z());
        sw.setNum(w());
        res = sx + " " + sy + " " + sz + " " + sw;
        return res;
    }

    static Quaternion fromString(const QString &s) {
        std::stringstream ss(s.toStdString());
        Real x,y,z,w;
        ss >> x >> y >> z >> w;
        return Quaternion(w,x,y,z);
    }

    Vector3 operator*(const Vector3 &a) const { return toRotationMatrix()*a; }
};


}

#endif // QUATERNION_H
